


import lejos.nxt.*;
import lejos.robotics.proposal.DifferentialPilot;
import lejos.robotics.subsumption.*;

public class LookForLine implements Behavior{
	private boolean suppressed = false;
	private DifferentialPilot pilot;
	private ColorLightSensor ultraSonicSensor;
	private int [] sensorValues = new int[4]; //array waar de leeswaarden van de sensor in worden opgeslagen
	
	protected static final int RGB_RED = 0;
	protected static final int RGB_GREEN = 1;
	protected static final int RGB_BLUE = 2;
	
	public LookForLine(ColorLightSensor ultraSonicSensor, DifferentialPilot pilot){
		this.ultraSonicSensor = ultraSonicSensor;
		this.pilot = pilot;
	}
	
	@Override
	public boolean takeControl(){
		//Er moet gezocht worden naar de lijn eens de sensor de zwarte lijn niet meer ziet
		//Dit betekent dat de sensor het wit van de grond ziet => (gecalibreerde) value < 50. 
		//Dit is als de sensor zo gecalibreerd is dat zwart == 100
		return !checkIfSimilarToBlack(sensorValues);
	}	
	
	@Override
	public void action(){
		suppressed = false;		
		while(!suppressed){
			int angle = 5;
			boolean lineFound = false;
			while(!lineFound){
				//TODO: draait nu nog maar naar 1 kant, andere kant draaien nog implementeren
				pilot.rotate(angle);
				lineFound = checkIfSimilarToBlack(sensorValues);
				angle += 5;
			}
			//lijn is gevonden, robot is weer op juiste weg. Uit while lus springen
			suppress();
		}	
	}
	
	@Override
	public void suppress(){
		pilot.stop();
		suppressed = true;
	}
	
	private boolean checkIfSimilarToBlack(int [] sensorValues){
		//TODO: iets te simplistische check atm. Uitproberen zal wel iets beters aanwijzen
		//RGB van zwart is [0, 0, 0] en beige is [245, 245, 220]
		ultraSonicSensor.readValues(sensorValues);
		return sensorValues[RGB_RED] < 125 && sensorValues[RGB_GREEN] < 125 && sensorValues[RGB_BLUE] < 125;
		
	}

}
